% CARP
% Author: Vojtech Novak
% Email: novakvo2@fel.cvut.cz
% -----------------------------

clear;
clc;

load data_graf4.mat
WeightMatrix = edges2matrixparam(g);
%DemandMatrix = ones( size(WeightMatrix) ); % tady muze byt
                                            % DemandMatrix = WeightMatrix
DemandMatrix = WeightMatrix;
                                            
                                            
graf = WeightMatrix;

W = 30
demand = 1;
deponode = 1;

pocet_hran = 0;

for i=1:size(graf,1)   
    for j=i+1:size(graf,2)
    
    %fprintf('EulerCircle by vertexes:\n');
    if ( graf(i,j) ~= Inf ) 
        pocet_hran = pocet_hran + 1;
    end
    
    
    end
end


% je vhodne vybudovat si strom nejratsich cest z DEPA, bodu 1
[minDistances roads] = findShortestPaths(WeightMatrix);



%Step1: Initialize - All demand arcs are services by a separate cycle -
%print them

INCMatrix = Weight2IncidenceMatrix( graf, pocet_hran ); % matice incidence

fprintf('\n//////////////////////////////////////////////////////\n');
fprintf('STEP 1:\n\n');

pocet_hran = size(INCMatrix, 2);

for i=1:pocet_hran

    [startnode, endnode] = StartEndIndex( INCMatrix(:, i)' );

    %k temto 2ma bodum ted musime najit nekratsi cestu
    
    p1 = roads( deponode, startnode ).vertexes;
    
    if ( isempty(p1) )
       p1 = [ deponode ]; 
    end
    
    p2 = roads( deponode, endnode ).vertexes;
    p2 = p2(end:-1:1);
    p = [ p1 p2 ];
    
    Cycles(i) = struct('edges', [ startnode, endnode ], 'route', p, 'cost', 0, 'demand', 0);
    Cycles(i).cost = costOfRoute( Cycles(i).route, WeightMatrix );
    Cycles(i).demand = demandOfRoute( Cycles(i).edges, DemandMatrix );  

end

%Cycles(1).edges(end+1, :) = [7 , 8];
%Cycles=Cycles(:,any(Cycles)); % smazani nulovych sloupecku

[unused, order] = sort([Cycles(:).cost]);
Cycles = Cycles( fliplr(order) ); 

%tisk
l = 0;
fprintf('No.\tHrana\tCesta\t\t\t\tCena\tDemand\n');
for i=1:pocet_hran
   
    fprintf('%d', i);
    fprintf('\t%d - %d\t', Cycles(i).edges(1, 1), Cycles(i).edges(1, 2) );
    pom1 = sprintf('%d-', Cycles(i).route);
    fprintf( pom1(1:end-1) );
    
    if ( l == 0 )
       l = length(pom1)-1; 
    end
    
    for x=1:(l - length(pom1)-1 )
       fprintf(' '); 
    end
    
    
    fprintf('\t\t %d', Cycles(i).cost );
    fprintf('\t%d', Cycles(i).demand );
    fprintf('\n');
    
end
fprintf('------------------------------------------------------\n');
fprintf('Total\t\t\t\t\t\t %d\t%d\n', sum(cell2mat({Cycles.cost})), sum(cell2mat({Cycles.demand})) );




fprintf('\n//////////////////////////////////////////////////////\n');
fprintf('STEP 2:\n\n');
%Step 2: Augument

odstranene_cykly = [];
cykl = Cycles(1);

for i=1:size(Cycles, 2)-1

    if ( Cycles(i).cost == -1 )
        continue;
    end
    
    
    for j=i+1:size(Cycles, 2)

        if ( Cycles(j).cost == -1 )
            continue;
        end

        if ( ( routeContainsEdge( Cycles(i).route, Cycles(j).edges ) == 1 ) && (Cycles(i).demand + Cycles(j).demand <= W) ) % TODO : dodelat podminku

            %muzu to zaugumentovat, tj. pridam hranu do vetsiho cyklu, zvysim
            %na nem demand a odstranim ten mensi cyklus
            Cycles(i).edges(end+1, :) = Cycles(j).edges;
            Cycles(i).demand = demandOfRoute( Cycles(i).edges, DemandMatrix ); % prepocet

            Cycles(j).cost = -1; % nastavit inactive
            odstranene_cykly = [ odstranene_cykly j ];
        end

    end

end

Cycles(odstranene_cykly) = [];
pocet_hran = size(Cycles, 2);

%tisk
l = 0;
fprintf('No.\tCesta\t\t\t\tCena\tDemand\n');
for i=1:pocet_hran
   
    fprintf('%d\t', i);
    pom1 = sprintf('%d-', Cycles(i).route);
    fprintf( pom1(1:end-1) );
    
    if ( l == 0 )
       l = length(pom1)-1; 
    end
    
    for x=1:(l - length(pom1)-1 )
       fprintf(' '); 
    end
    
    
    fprintf('\t\t %d', Cycles(i).cost );
    fprintf('\t%d', Cycles(i).demand );
    fprintf('\n');
    
end
fprintf('------------------------------------------------------\n');
fprintf('Total\t\t\t\t\t %d\t%d\n', sum(cell2mat({Cycles.cost})), sum(cell2mat({Cycles.demand})) );




fprintf('\n//////////////////////////////////////////////////////\n');
fprintf('STEP 3:\n\n');
%Step 3: Merge


for i=1:length(Cycles)
   
    for j=i+1:length(Cycles)
           
        if ( Cycles(i).demand + Cycles(j).demand > W )
           continue;    %pokud prekrocime W, pak nema cenu nic mergovat! 
        end
        
        tmp_c = mergeAll( Cycles(i), Cycles(j), WeightMatrix, DemandMatrix );
        if ( tmp_c.demand <= W && (tmp_c.demand == Cycles(i).demand + Cycles(j).demand) && (tmp_c.cost < Cycles(i).cost + Cycles(j).cost) )
            
            %merge se podaril
            odstranene_cykly = [ i j ];
            Cycles(odstranene_cykly) = [];          % smazani tech 2 stavajicich
            Cycles( length(Cycles) + 1 ) = tmp_c;   % nahrazeni novym
            
            [unused, order] = sort([Cycles(:).cost]); % sort
            Cycles = Cycles( fliplr(order) );
            
            % a jedeme znova merging, dokud se neprestane mergovat ...
            i = 1;
            break;            
            
        end
                                
    end
   
end

pocet_hran = size(Cycles, 2);

%tisk
l = 0;
fprintf('No.\tCesta\t\t\t\tCena\tDemand\n');
for i=1:pocet_hran
   
    fprintf('%d\t', i);
    pom1 = sprintf('%d-', Cycles(i).route);
    fprintf( pom1(1:end-1) );
    
    if ( l == 0 )
       l = length(pom1)-1; 
    end
    
    for x=1:(l - length(pom1)-1 )
       fprintf(' '); 
    end
    
    
    fprintf('\t\t %d', Cycles(i).cost );
    fprintf('\t%d', Cycles(i).demand );
    fprintf('\n');
    
end
fprintf('------------------------------------------------------\n');
fprintf('Total\t\t\t\t\t %d\t%d\n', sum(cell2mat({Cycles.cost})), sum(cell2mat({Cycles.demand})) );




